/**************************************************************************************************
* Title: SumVector2.cpp
* Author: Gael Huber
* Description: Data structure containing 2-dimensional vector and its various operations.
**************************************************************************************************/
#include "SumMath.h"

/**************************************************************************************************
* Hermite interpolation between position V1, tangent T1 (when s == 0) and position V2, tangent T2 
* (when s == 1).
**************************************************************************************************/
Vector2* Vec2Hermite(Vector2* pOut, const Vector2* pV1, const Vector2* pT1, const Vector2* pV2, 
	const Vector2* pT2, float s) {
	// Calculate a, b, c, and d values for formula
	float a(2.0f*s*s*s - 3.0f*s*s + 1.0f);
	float b(-2.0f*s*s*s + 3.0f*s*s);
	float c(s*s*s - 2.0f*s*s + s);
	float d(s*s*s - s*s);
	
	// Solve for a*v1 + b*v2 + c*t1 + d*t2
	Vector2 v1, v2;
	
	Vec2Scale(&v1, pV1, a);
	Vec2Scale(&v2, pV2, b);
	Vec2Add(pOut, &v1, &v2);

	Vec2Scale(&v1, pT1, c);
	Vec2Add(pOut, pOut, &v1);

	Vec2Scale(&v1, pT2, d);
	Vec2Add(pOut, pOut, &v1);	

	return pOut;
}

/**************************************************************************************************
* CatmullRom interpolation between V1 (when s == 0) and V2 (when s == 1)
**************************************************************************************************/
Vector2* Vec2CatmullRom(Vector2* pOut, const Vector2* pV0, const Vector2* pV1, const Vector2* pV2, 
	const Vector2* pV3, float s) {
	// Calculate a, b, c, d
	float a(-s*s*s + 2.0f*s*s - s);
	float b(3.0f*s*s*s - 5.0f*s*s + 2.0f);
	float c(-3.0f*s*s*s + 4.0f*s*s + s);
	float d(s*s*s - s*s);

	// Solve for (a*p1 + b*p2 + c*p3 + d*p4)/2
	Vector2 v1, v2;

	Vec2Scale(&v1, pV0, a);
	Vec2Scale(&v2, pV1, b);
	Vec2Add(pOut, &v1, &v2);

	Vec2Scale(&v1, pV2, c);
	Vec2Add(pOut, pOut, &v1);

	Vec2Scale(&v1, pV3, d);
	Vec2Add(pOut, pOut, &v1);

	Vec2Scale(pOut, pOut, 0.5f);

	return pOut;
}

/**************************************************************************************************
* Barycentric coordinates V1 + f(V2 - V1) + g(V3 - V1)
**************************************************************************************************/
Vector2* Vec2Barycentric(Vector2* pOut, const Vector2* pV1, const Vector2* pV2, const Vector2* pV3,
	float f, float g) {
	*pOut = *pV1 + f * (*pV2 - *pV1) + g * (*pV3 - *pV1);
	return pOut;
}

/**************************************************************************************************
* Transform by matrix (x, y, 0, 1)
**************************************************************************************************/
Vector4* Vec2Transform(Vector4* pOut, const Vector2* pV, const Matrix* pM) {
	pOut->x = pM->_11*pV->x + pM->_12*pV->y + pM->_14;
	pOut->y = pM->_21*pV->x + pM->_22*pV->y + pM->_24;
	pOut->z = pM->_31*pV->x + pM->_32*pV->y + pM->_34;
	pOut->w = pM->_41*pV->x + pM->_42*pV->y + pM->_44;
	return pOut;
}

/**************************************************************************************************
* Transform coords by matrix (x, y, 0, 1)
**************************************************************************************************/
Vector2* Vec2TransformCoord(Vector2* pOut, const Vector2* pV, const Matrix* pM) {
	pOut->x = pM->_11*pV->x + pM->_12*pV->y + pM->_14;
	pOut->y = pM->_21*pV->x + pM->_22*pV->y + pM->_24;
	return pOut;
}

/**************************************************************************************************
* Transform normal by matrix (x, y, 0, 0)
**************************************************************************************************/
Vector2* Vec2TransformNormal(Vector2* pOut, const Vector2* pV, const Matrix* pM) {
	pOut->x = pM->_11*pV->x + pM->_12*pV->y;
	pOut->y = pM->_21*pV->x + pM->_22*pV->y;
	return pOut;
}